﻿#include "park.h"

#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "app_main.h"
#include "drv_misc.h"
#include "elog.h"
#include "math.h"
#include "net_opt.h"
#include "os_sys.h"
#include "param.h"
#include "radar.h"
#include "vz_log.h"

#define GET_RADAR_STATE(x) ((x & 0xF0) >> 4)
#define GET_ALG_STATE(x) (x & 0x0F)

static SocPowerCb g_cbSocPowFunc = NULL;
static TriggerInfo_t g_stTrigInfo = {0};
static radar_history_t g_stRadarData = {0};
static OS_MSGQUE g_queueCmd = NULL;
static int g_iLastPipeStates = -1;
static bool g_bFactoryInit = false;

/*轮询计时器使能*/
static void PollingTimer_Enable(PollingTimer_t *timer, bool enable) {
  timer->enable = enable;
}

/*轮询计时器配置超时间隔*/
static void PollingTimer_Interval(PollingTimer_t *timer, uint64_t interval) {
  timer->interval = interval;
}

/*轮询计时器重置*/
static void PollingTimer_Restart(PollingTimer_t *timer, uint64_t cur_time) {
  timer->ever_time_out = true;
  timer->last_timeout = cur_time;
}

/*轮询计时器检测，返回true表示超时*/
static bool PollingTimer_Probe(PollingTimer_t *timer, uint64_t cur_time,
                               bool alarm) {
#define DAY_TO_SEC(y) (y * 24 * 60 * 60)
  uint64_t time_slice = 0;
  if ((!timer->enable) || (!alarm)) {
    timer->last_timeout = 0;
    return false;
  }
  if ((!timer->ever_time_out) && (alarm)) {
    timer->ever_time_out = true;
    return true;
  }

  // 如果时间相差太大，说明进行了ntp校时
  if (cur_time - timer->last_timeout > DAY_TO_SEC(1)) {
    // 手动同步上次超时的时间
    timer->last_timeout = cur_time - DrvMisc_GetSysTick();
  }

  if (cur_time >= timer->last_timeout) {
    time_slice = cur_time - timer->last_timeout;
  } else {
    // time_slice = cur_time + (((uint64_t)-1) / 1000) - timer->last_timeout;
    time_slice = cur_time + ((uint64_t)-1) - timer->last_timeout;
  }
  if (time_slice > timer->interval) {
    return true;
  }
  return false;
}

void ResetDataHistory(tirggerHistory *history) {
  history->triggerCount = 0;
  history->lastTriggerDist = -1;
  history->lastTriggerMean = 0.f;
  history->lastTriggerStd = 0.f;
  history->bObjStableMode = false;
}

static void AddSocOnTimeout(ParkCtx_t *ctx, uint32_t timeout) {
  ctx->soc_on_timeout += timeout;
}

static void ResetSocOnTimeout(ParkCtx_t *ctx) {
  ctx->soc_on_timeout = SOCON_TIMEOUT_IN_S;
}

/*业务初始化*/
int Park_Init(ParkCtx_t *ctx, ParamInfo_t *p) {
  uint64_t cur_time = 0;
  memset(ctx, 0, sizeof(ParkCtx_t));
  ctx->soc_on_timeout = SOCON_TIMEOUT_IN_S;
  /*配置默认状态，默认空场，第一次上电*/
  ctx->first_poweron = true;
  ctx->park_state = PARK_TYPE_FREE | (PARK_TYPE_FREE << 4);
  ctx->radar_proc.radarFreeDist = 580;
  ResetDataHistory(&ctx->radar_proc.triggerHistory);

  if (Param_IsFactoryMode(p)) {
    ctx->state = PIPE_STATE_FACTORY;
  }
  // cur_time = OS_GETTICKCNT() / 1000;
  DrvMisc_GetSec(&cur_time);

  /*心跳超时计时器重置*/
  PollingTimer_Restart(&ctx->heart_timer, cur_time);
  PollingTimer_Restart(&ctx->wdt_timer, cur_time);
  return 0;
}

/*业务销毁*/
int Park_Unint(ParkCtx_t *ctx) { return 0; }

/*业务销毁*/
static int Probe_FirstPowerOn(bool *first) {
  if (*first == false) {
    return TRIG_TYPE_NO;
  }
  *first = false;
  return TRIG_TYPE_PWRON;
}

/*超时心跳检测*/
static int Probe_Heart(PollingTimer_t *timer, ParamInfo_t *p,
                       uint64_t time_now) {
  PollingTimer_Enable(timer, p->info.heart_interval != 0);
  PollingTimer_Interval(timer, p->info.heart_interval);
  if (PollingTimer_Probe(timer, time_now, true)) {
    log_i("battery door open alarm, interval %ds", p->info.heart_interval);
    return TRIG_TYPE_HEART;
  }
  return TRIG_TYPE_NO;
}

static int Probe_DebugMode(ParkCtx_t *ctx, ParamInfo_t *p, uint64_t time_now) {
  int event = 0;
  if (ctx->first_poweron) {
    event |= TRIG_TYPE_DEBUG;
    return event;
  }

  if (DrvMisc_IsDebug()) {
    DrvMisc_ClearRestoreFlag();
    event |= TRIG_TYPE_DEBUG;
  } else if (DrvMisc_IsRestore()) {
    DrvMisc_ClearRestoreFlag();
    Param_SetAsDefault();
    Param_Save();
    event |= TRIG_TYPE_RESOTRE;
  } else if ((ctx->debug_pending != 0) && (ctx->state == PIPE_STATE_DETECT)) {
    event |= ctx->debug_pending;
    ctx->debug_pending = 0;
  }

  return event;
}

/*车牌确认事件*/
static int Probe_Plate(PollingTimer_t *timer, ParamAdvInfo_t *p_adv,
                       uint32_t *plate_event, uint64_t time_now) {
  PollingTimer_Enable(timer, *plate_event == 1);
  PollingTimer_Interval(timer, p_adv->info.plate_event_interval);
  if (PollingTimer_Probe(timer, time_now, p_adv->info.plate_event_interval)) {
    (*plate_event)++;
    log_i("plate event, interval %ds", p_adv->info.plate_event_interval);
    return TRIG_TYPE_PLATE;
  }
  return 0;
}

/*电池门报警检测*/
static int Probe_Battdoor(PollingTimer_t *timer, ParamInfo_t *p,
                          uint64_t time_now) {
  static bool alarm = false;
  bool door_open = DrvMisc_IsBattDoorOpen();
  /*如果报警间隔为0，只有当门未关并且没有报警过才报警*/
  bool alarm_once = door_open && (!alarm);
  if ((alarm) && (!door_open)) {
    alarm = false;
  }
  PollingTimer_Enable(timer, p->info.batt_door_alarm_en != 0);
  PollingTimer_Interval(timer, p->info.batt_door_alarm_interval);
  if (PollingTimer_Probe(timer, time_now,
                         (p->info.batt_door_alarm_interval == 0) ? alarm_once
                                                                 : door_open)) {
    alarm = true;
    PollingTimer_Restart(timer, time_now);
    log_i("battery door open alarm, interval %ds",
          p->info.batt_door_alarm_interval);
    return TRIG_TYPE_BATT_DOOR;
  }
  return TRIG_TYPE_NO;
}

/*电量低检测*/
static int Probe_BattLow(PollingTimer_t *timer, ParamInfo_t *p,
                         uint64_t time_now, bool *protect) {
  static bool alarm = false;
  uint8_t bat_cap = 0;
  uint16_t bat_volt = 0;
  DrvMisc_GetBattInfoByQV(&bat_cap, &bat_volt);
  bool batt_low = (bat_cap < p->info.batt_low_thresh) &&
                  (bat_cap > p->info.batt_protect_thresh);
  bool alarm_once = batt_low && (!alarm);
  if ((alarm) && (!batt_low)) {
    alarm = false;
  }
  bool batt_protect = (bat_cap <= p->info.batt_protect_thresh);
  if (batt_protect) {
    log_i("battery low(cap %d < protect %d)", bat_cap,
          p->info.batt_protect_thresh);
    *protect = true;
  } else if (batt_low) {
    log_i("battery low(cap %d < alarm %d), interval %d", bat_cap,
          p->info.batt_low_thresh, p->info.batt_low_alarm_interval);
    PollingTimer_Enable(timer, p->info.batt_low_alarm_en != 0);
    PollingTimer_Interval(timer, p->info.batt_low_alarm_interval);
    if (PollingTimer_Probe(timer, time_now,
                           (p->info.batt_low_alarm_interval == 0) ? alarm_once
                                                                  : batt_low)) {
      PollingTimer_Restart(timer, time_now);
      alarm = true;
      log_i("battery low alarm, interval %ds", p->info.batt_low_alarm_interval);
      return TRIG_TYPE_LOW_BATT;
    }
  }

  return TRIG_TYPE_NO;
}

/*雷达数据插入缓存*/
void BuffInsert(DataBuff_t *buffer, RadarData_t *newData) {
  if (buffer->count < RADAR_DATA_BUFF) {
    buffer->data[buffer->count].dist = newData->dist;
    buffer->data[buffer->count].amp = newData->amp;
    buffer->count++;
    return;
  }
  buffer->data[buffer->front].dist = newData->dist;
  buffer->data[buffer->front].amp = newData->amp;
  buffer->front =
      (buffer->front == RADAR_DATA_BUFF - 1) ? 0 : (buffer->front + 1);
}

/*获得缓存中雷达数据数量*/
int BuffGetCount(DataBuff_t *buffer) { return buffer->count; }

/*获得缓存中指定位置的雷达数据*/
RadarData_t *BufferAt(DataBuff_t *buffer, int index) {
  int pos = 0;
  if (index > (buffer->count - 1)) {
    return NULL;
  }
  if (buffer->count < RADAR_DATA_BUFF) {
    pos = buffer->count - 1 - index;
  } else {
    if (buffer->front == 0) {
      pos = RADAR_DATA_BUFF - 1 - index;
    } else if (buffer->front - 1 >= index) {
      pos = buffer->front - 1 - index;
    } else {
      pos = buffer->front - 1 + RADAR_DATA_BUFF - index;
    }
  }
  return &buffer->data[pos];
}

/*雷达数据修改缓存*/
void BufferChange(DataBuff_t *buffer, RadarData_t *newData) {
  int idx = buffer->count - 1;
  if (buffer->count >= RADAR_DATA_BUFF && buffer->front > 0) {
    idx = buffer->front - 1;
  }
  buffer->data[idx].dist = newData->dist;
  buffer->data[idx].amp = newData->amp;
  log_i("radar idx: %d, changed:%d", idx, newData->dist);
}

/*雷达错误报警*/
static int Probe_RadarFault(PollingTimer_t *timer, ParamInfo_t *p,
                            uint64_t time_now, bool radar_fault) {
  static bool alarm = false;
  /*如果报警间隔为0，只有当雷达错误并且没有报警过才报警*/
  bool alarm_once = radar_fault && (!alarm);
  /*如果雷达正常且报过警，则清除报警信号*/
  if ((alarm) && (!radar_fault)) {
    alarm = false;
  }
  // PollingTimer_Enable(timer, p->info.radar_fault_alarm_en);
  PollingTimer_Enable(timer, true);
  PollingTimer_Interval(timer, p->info.radar_fault_alarm_interval);
  if (PollingTimer_Probe(timer, time_now,
                         (p->info.radar_fault_alarm_interval == 0)
                             ? alarm_once
                             : radar_fault)) {
    PollingTimer_Restart(timer, time_now);
    log_i("radar fault alarm, interval %ds",
          p->info.radar_fault_alarm_interval);
    alarm = true;
    return TRIG_TYPE_RADAR_FAULT;
  }

  return TRIG_TYPE_NO;
}

/*入场时间超时*/
static int Probe_ParkInTimeout(PollingTimer_t *timer, uint32_t interval,
                               uint64_t time_now) {
  /*雷达数据测试不启用超时策略*/
#ifndef TESTRADARDATABASE
  PollingTimer_Enable(timer, true);
  PollingTimer_Interval(timer, interval);
  if (PollingTimer_Probe(timer, time_now, true)) {
    return TRIG_TYPE_PARK;
  }
#endif
  return TRIG_TYPE_NO;
}

/*出场时间超时*/
static int Probe_ParkOutTimeout(PollingTimer_t *timer, uint32_t interval,
                                uint64_t time_now) {
#ifndef TESTRADARDATABASE
  PollingTimer_Enable(timer, true);
  PollingTimer_Interval(timer, interval);
  if (PollingTimer_Probe(timer, time_now, true)) {
    return TRIG_TYPE_PARK;
  }
#endif
  return TRIG_TYPE_NO;
}

/*雷达检测*/
static int RadarDetect(ParkCtx_t *ctx, ParamInfo_t *p) {
  RadarData_t radar_value = {0};
  int ret = 0;
  RadarProc_t *proc = &ctx->radar_proc;
  /*检测模式*/
  if (true) {
    ret = Radar_Trigger(&radar_value);
    if (ret == 0) {
      /*数据插入buffer*/
      BuffInsert(&proc->buff, &radar_value);
      memcpy(&ctx->last_radar, &radar_value, sizeof(RadarData_t));
    }
  }
  return ret;
}

// static bool radar_change_fast(ParkCtx_t *ctx, ParamInfo_t *p) {
//   RadarProc_t *proc = &ctx->radar_proc;
//
//   RadarData_t *radar_dist = BufferAt(&proc->buff, 0);
//   if (radar_dist == NULL) return false;
//   if (radar_data_is_valid(radar_dist) && radar_data_is_change(&proc->buff,
//   p)) {
//     /*如果雷达数据发生变化，则进入多次检测模式*/
//     return true;
//   }
//   return false;
// }

static bool isMaxInvalidRadarData(RadarData_t *radar_dist) {
  if (radar_dist->dist == 0 &&
      (radar_dist->amp < 200 || radar_dist->amp > RADAR_AMP_UP_THRESHOLD)) {
    return true;
  } else if (radar_dist->dist == 900) {
    return true;
  }
  return false;
}

// static void car_change_prob(RadarProc_t *proc, ParamInfo_t *p) {
//   proc->event_count++;
//   if (proc->event_count < 3) {
//     proc->isCarChanged = false;
//     return;
//   }
//   RadarData_t *radar_dist[3] = {0};
//   for (int i = 0; i < 3; i++) {
//     radar_dist[i] = BufferAt(&proc->buff, 0);
//     if (!radar_dist[i]) {
//       proc->isCarChanged = false;
//       return;
//     }
//     if (((radar_dist[i]->dist < p->info.radar_exit_dist) &&
//          (radar_dist[i]->amp > 200)) ||
//         (isMaxInvalidRadarData(radar_dist[i]))) {
//       proc->isCarChanged = false;
//       return;
//     }
//   }
//   proc->isCarChanged = true;
// }

static bool getValidDist(RadarData_t **radar_dist, DataBuff_t *buff,
                         int need_count, int *get_count,
                         int *invalidRadarCount) {
  *get_count = 0;
  *invalidRadarCount = 0;
  int invalidRadarData = 0;
  if (need_count <= 0) {
    return false;
  }
  for (int i = 5; i < RADAR_DATA_BUFF; ++i) {
    RadarData_t *nowRadarDist = BufferAt(buff, i);
    if (!nowRadarDist) {
      return false;
    }
    if (isMaxInvalidRadarData(nowRadarDist)) {
      invalidRadarData++;
      *invalidRadarCount = *invalidRadarCount + 1;
      if (invalidRadarData >= RADAR_MAX_ZERO_FREE_COUNT) {
        return false;
      }
    } else {
      invalidRadarData = 0;
      radar_dist[*get_count] = nowRadarDist;
      (*get_count) += 1;
      if (*get_count >= need_count) {
        return true;
      }
    }
  }
  return false;
}

static bool getStd(DataBuff_t *buff, int dealCount, float *std, float *mean,
                   int *invalidRadarCount, int *getCount) {
  *std = 0.f;
  *mean = 0.f;
  *getCount = 0;
  RadarData_t *radar_dist_list[RADAR_MAX_STD_CALC_COUNT];
  int needCount = dealCount >= RADAR_MAX_STD_CALC_COUNT
                      ? RADAR_MAX_STD_CALC_COUNT
                      : dealCount;
  int radar_dist_list_size = 0;
  float tmpValue = 0.f;
  getValidDist(radar_dist_list, buff, needCount, &radar_dist_list_size,
               invalidRadarCount);
  *getCount = radar_dist_list_size;
  int total = 0;
  for (int i = 0; i < radar_dist_list_size; ++i) {
    total += radar_dist_list[i]->dist;
  }
  if (radar_dist_list_size > 0) {
    *mean = 1.f * total / radar_dist_list_size;

    for (int i = 0; i < radar_dist_list_size; ++i) {
      tmpValue = radar_dist_list[i]->dist - *mean;
      *std += tmpValue * tmpValue;
    }
    float result = sqrtf((*std) / radar_dist_list_size);
    *std = result;
  }
  if (radar_dist_list_size >= RADAR_MAX_STD_CALC_COUNT) {
    return true;
  }
  return false;
}

int getThresholdBase(int updateThreshold, ParamInfo_t *p) {
  int useThreshold = 580;
  if (updateThreshold > useThreshold * 0.7f &&
      useThreshold < useThreshold * 1.3f) {
    useThreshold = updateThreshold;
  }
  return useThreshold;
}

static bool isInHeighLevelDist(ParkCtx_t *ctx, int dist, ParamInfo_t *p) {
  // int threshold = p->info.radar_enter_dist * 0.8f;
  int threshold = getThresholdBase(ctx->radar_proc.radarFreeDist, p) * 0.8f;
  if (dist >= threshold) {
    return true;
  }
  return false;
}

static bool isInMiddleLevelDist(ParkCtx_t *ctx, int dist, ParamInfo_t *p) {
  // int highThreshold = p->info.radar_enter_dist * 0.8f;
  // int lowThreshold = p->info.radar_enter_dist * 0.6f;
  int highThreshold = getThresholdBase(ctx->radar_proc.radarFreeDist, p) * 0.8f;
  int lowThreshold = getThresholdBase(ctx->radar_proc.radarFreeDist, p) * 0.5f;
  if (dist >= lowThreshold && dist < highThreshold) {
    return true;
  }
  return false;
}

static bool isInLowLevelDist(ParkCtx_t *ctx, int dist, ParamInfo_t *p) {
  int lowThreshold = getThresholdBase(ctx->radar_proc.radarFreeDist, p) * 0.5f;
  if (dist < lowThreshold) {
    return true;
  }
  return false;
}

static bool isStableObjMode(bool bStdMode, int now_dist, float mean,
                            float std) {
  if (bStdMode) {
    if (std > 20.f) {
      return false;
    }
    float diff = now_dist - mean;
    float threshold = RADAR_STD_RATIO * std;
    if (threshold < RADAR_MIN_STD_THRESHOLD) {
      threshold = RADAR_MIN_STD_THRESHOLD;
    }
    if (diff > -threshold && diff < threshold) {
      return true;
    }
  }
  return false;
}

// 雷达值是否在长时间遮挡雷达范围内
static bool isInStableObjArea(ParkCtx_t *ctx, uint16_t radarDist) {
  float useMean = ctx->radar_proc.triggerHistory.lastTriggerMean;
  float useStd = ctx->radar_proc.triggerHistory.lastTriggerStd;
  if (useStd > 20) {
    useMean = ctx->radar_proc.triggerHistory.lastTriggerDist;
    useStd = 20;
  }
  int useCount =
      ctx->radar_proc.triggerHistory.triggerCount > RADAR_MAX_TRIGGER_COUNT
          ? RADAR_MAX_TRIGGER_COUNT
          : ctx->radar_proc.triggerHistory.triggerCount;
  float countRatio = 1.f * useCount / RADAR_MAX_TRIGGER_COUNT + 1.f;

  float useThreshold = RADAR_STD_RATIO * useStd;
  if (useThreshold < RADAR_MIN_STD_THRESHOLD) {
    useThreshold = RADAR_MIN_STD_THRESHOLD;
  }

  if (radarDist >= useMean - useThreshold * countRatio &&
      radarDist <= useMean + useThreshold * countRatio) {
    return true;
  }
  return false;
}

static bool isFreeParkTriggerEvent(ParkCtx_t *ctx, ParamInfo_t *p) {
  float std = 0.f;
  float mean = 0.f;
  int invalidRadarCount = 0;
  int std_count = 0;
  RadarData_t *now_radar_dist = NULL;
  now_radar_dist = BufferAt(&ctx->radar_proc.buff, 0);
  if (!now_radar_dist)
    return false;
  if (isMaxInvalidRadarData(now_radar_dist)) {
    if (ctx->radar_proc.stableZeroCount >= RADAR_MAX_ZERO_FREE_COUNT) {
      ctx->radar_proc.freeUnstableCount = 0;
      return false;
    } else {
      for (int i = 0; i < RADAR_MAX_TRIGGER_COUNT; ++i) {
        RadarData_t tmpRadarData;
        int ret = Radar_Trigger(&tmpRadarData);
        if (ret == 0) {
          if (!isMaxInvalidRadarData(&tmpRadarData)) {
            BufferChange(&ctx->radar_proc.buff, &tmpRadarData);
            break;
          }
        }
      }

      now_radar_dist = BufferAt(&ctx->radar_proc.buff, 0);
      if (!now_radar_dist)
        return false;
      if (isMaxInvalidRadarData(now_radar_dist)) {
        ctx->radar_proc.stableZeroCount++;
        if (ctx->radar_proc.stableZeroCount >= RADAR_MAX_ZERO_FREE_COUNT) {
          ctx->radar_proc.freeUnstableCount = 0;
          ctx->radar_proc.radarFreeDist = 0;
        }
        if (ctx->radar_proc.triggerHistory.bObjStableMode) {
          ctx->radar_proc.triggerHistory.bObjStableMode = false;

          LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_REASON,
                        "skip so mode %d %d", now_radar_dist->dist,
                        ctx->radar_proc.triggerHistory.lastTriggerMean);
        }
        return false;
      }
    }
  }
  bool bStdMode = false;
  if (getStd(&ctx->radar_proc.buff, ctx->radar_proc.parkStableCount, &std,
             &mean, &invalidRadarCount, &std_count)) {
    bStdMode = true;
  }

  if (ctx->radar_proc.triggerHistory.bObjStableMode) {
    if (!isInStableObjArea(ctx, now_radar_dist->dist)) {
      ctx->radar_proc.triggerHistory.bObjStableMode = false;

      LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_REASON, "skip so mode %d %d",
                    now_radar_dist->dist,
                    ctx->radar_proc.triggerHistory.lastTriggerMean);
    }
  }

  if (isInHeighLevelDist(ctx, now_radar_dist->dist, p)) {
    if (bStdMode) {
      if (isInHeighLevelDist(ctx, mean, p) && std < 40) {
        ctx->radar_proc.radarFreeDist =
            (mean - (RADAR_STD_RATIO + 1) * std) / 0.8f;
      }
    }
    ctx->radar_proc.freeUnstableCount = 0;
    return false;
  } else if (isInLowLevelDist(ctx, now_radar_dist->dist, p)) {
    // 此处防误触发应区分长时间遮挡和短时间遮挡
    if (!ctx->radar_proc.isCarChanged &&
        ctx->radar_proc.triggerHistory.triggerCount > 0) {
      if (ctx->radar_proc.triggerHistory.bObjStableMode) {
        // 防长时间遮挡误触发
        if (isInStableObjArea(ctx, now_radar_dist->dist)) {
          return false;
        }
      } else {
        // 防短时间遮挡误触发
        ctx->radar_proc.freeUnstableCount++;
        int useCount = ctx->radar_proc.triggerHistory.triggerCount >
                               RADAR_MAX_PROTECT_COUNT
                           ? RADAR_MAX_PROTECT_COUNT
                           : ctx->radar_proc.triggerHistory.triggerCount;

        if (ctx->radar_proc.freeUnstableCount <= useCount) {
          return false;
        }
      }
    }
    ctx->radar_proc.triggerHistory.triggerCount += 1;
    ctx->radar_proc.triggerHistory.lastTriggerDist = now_radar_dist->dist;
    ctx->radar_proc.triggerHistory.lastTriggerMean = now_radar_dist->dist;
    ctx->radar_proc.triggerHistory.lastTriggerStd = 20;
    LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_REASON, "free 1(%d %d %d)",
                  ctx->radar_proc.triggerHistory.bObjStableMode,
                  ctx->radar_proc.freeUnstableCount,
                  ctx->radar_proc.isCarChanged);
    ctx->radar_proc.triggerHistory.bObjStableMode =
        isStableObjMode(bStdMode, now_radar_dist->dist, mean, std);

    return true;
  }
  if (bStdMode) {
    float diff = now_radar_dist->dist - mean;
    float threshold = RADAR_STD_RATIO * std;
    if (threshold < RADAR_MIN_STD_THRESHOLD) {
      threshold = RADAR_MIN_STD_THRESHOLD;
    }
    if (diff <= -threshold || diff >= threshold) {
      // 此处防误触发考虑短时间遮挡，误触发次数越多入场需要等待的时间越长，最长为RADAR_MAX_PROTECT_COUNT+1秒
      ctx->radar_proc.freeUnstableCount++;
      int useCount =
          ctx->radar_proc.triggerHistory.triggerCount > RADAR_MAX_PROTECT_COUNT
              ? RADAR_MAX_PROTECT_COUNT
              : ctx->radar_proc.triggerHistory.triggerCount;

      if (ctx->radar_proc.freeUnstableCount > useCount) {
        bool bTrigger = false;
        uint16_t tmpRecord[RADAR_MAX_TRIGGER_COUNT] = {0};
        for (int i = 0; i < RADAR_MAX_TRIGGER_COUNT; ++i) {
          RadarData_t tmpRadarData;
          int ret = Radar_Trigger(&tmpRadarData);
          if (ret == 0) {
            tmpRecord[i] = tmpRadarData.dist;
            if (!isMaxInvalidRadarData(&tmpRadarData)) {
              float diff = tmpRadarData.dist - mean;
              if (diff <= -threshold || diff >= threshold) {
                bTrigger = true;
                break;
              }
            }
          }
        }
        if (bTrigger) {
          ctx->radar_proc.triggerHistory.triggerCount += 1;
          ctx->radar_proc.triggerHistory.lastTriggerDist = now_radar_dist->dist;
          ctx->radar_proc.triggerHistory.lastTriggerMean = mean;
          ctx->radar_proc.triggerHistory.lastTriggerStd = std;
          ctx->radar_proc.triggerHistory.bObjStableMode = false;
          LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_REASON,
                        "free 2 %d %d %d %d %d", tmpRecord[0], tmpRecord[1],
                        tmpRecord[2], tmpRecord[3], tmpRecord[4]);
          return true;
        }
      }
    } else {
      ctx->radar_proc.freeUnstableCount = 0;
    }
    return false;
  } else {
    if (ctx->radar_proc.parkStableCount <= RADAR_MAX_STD_CALC_COUNT * 2) {
      // 刚从其他状态切换到空闲30帧左右时
      // 不用均值判断，小于入场距离才入场
      ctx->radar_proc.freeUnstableCount = 0;
      return false;
    } else {
      // 空闲时打到0的情况会出现在这里
      // 此处防误触发应该考虑短时间遮挡
      int validRadarDataCount = 0;
      // 先确认是否为有效数据
      uint16_t tmpRecord[RADAR_MAX_TRIGGER_COUNT] = {0};
      for (int i = 0; i < RADAR_MAX_TRIGGER_COUNT; ++i) {
        RadarData_t tmpRadarData;
        int ret = Radar_Trigger(&tmpRadarData);
        if (ret == 0) {
          tmpRecord[i] = tmpRadarData.dist;
          if (!isMaxInvalidRadarData(&tmpRadarData)) {
            if (!isInHeighLevelDist(ctx, tmpRadarData.dist, p)) {
              validRadarDataCount++;
            }
          }
        }
      }
      if (validRadarDataCount > RADAR_MAX_TRIGGER_COUNT / 2) {
        // 为有效数据时再防止误触发，误触发次数越多入场需要等待的时间越长，最长为4秒
        ctx->radar_proc.freeUnstableCount++;
        int useCount = ctx->radar_proc.triggerHistory.triggerCount >
                               RADAR_MAX_PROTECT_COUNT
                           ? RADAR_MAX_PROTECT_COUNT
                           : ctx->radar_proc.triggerHistory.triggerCount;

        if (ctx->radar_proc.freeUnstableCount > useCount) {
          ctx->radar_proc.triggerHistory.triggerCount += 1;
          ctx->radar_proc.triggerHistory.lastTriggerDist = now_radar_dist->dist;
          ctx->radar_proc.triggerHistory.lastTriggerMean = now_radar_dist->dist;
          ctx->radar_proc.triggerHistory.lastTriggerStd = 20;
          ctx->radar_proc.triggerHistory.bObjStableMode = false;
          LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_REASON,
                        "free 3 %d %d %d %d %d", tmpRecord[0], tmpRecord[1],
                        tmpRecord[2], tmpRecord[3], tmpRecord[4]);
          return true;
        }
      }
      return false;
    }

    return false;
  }
  return false;
}

static bool isInParkTriggerEvent(ParkCtx_t *ctx, ParamInfo_t *p,
                                 bool bOverHalfTime) {
  RadarData_t *now_radar_dist = NULL;
  float std = 0.f;
  float mean = 0.f;
  int invalidRadarCount = 0;
  int std_count = 0;
  now_radar_dist = BufferAt(&ctx->radar_proc.buff, 0);
  if (!now_radar_dist)
    return false;

  if (isMaxInvalidRadarData(now_radar_dist)) {
    // ctx->radar_proc.inStableCount = 0;
    if (ctx->radar_proc.radarFreeDist == 0) {
      // ctx->radar_proc.isCarChanged = true;
    }
    return false;
  }

  if (isInHeighLevelDist(ctx, now_radar_dist->dist, p)) {
    ctx->radar_proc.isCarChanged = true;
    LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_REASON, "in 1");
    return true;
  }
  if (!bOverHalfTime) {
    return false;
  }
  if (getStd(&ctx->radar_proc.buff, ctx->radar_proc.parkStableCount, &std,
             &mean, &invalidRadarCount, &std_count)) {
    if (std < 4) {
      LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_REASON, "in 2");
      return true;
    } else
      return false;
  } else {
    return false;
  }
  return false;
}

static bool isStableParkTriggerEvent(ParkCtx_t *ctx, ParamInfo_t *p,
                                     uint32_t time_now) {
  RadarData_t *now_radar_dist = NULL;
  float std = 0.f;
  float mean = 0.f;
  int invalidRadarCount = 0;
  int std_count = 0;
  now_radar_dist = BufferAt(&ctx->radar_proc.buff, 0);

  int useCount =
      ctx->radar_proc.triggerHistory.triggerCount > RADAR_MAX_TRIGGER_COUNT
          ? RADAR_MAX_TRIGGER_COUNT
          : ctx->radar_proc.triggerHistory.triggerCount;
  float countRatio = 1.f * useCount / RADAR_MAX_TRIGGER_COUNT + 1.f;

  if (!now_radar_dist)
    return false;

  bool bStdMode = false;
  bool bBlackCarMode = false;
  if (getStd(&ctx->radar_proc.buff, ctx->radar_proc.parkStableCount, &std,
             &mean, &invalidRadarCount, &std_count)) {
    bStdMode = true;
  } else if (ctx->radar_proc.parkStableCount >= RADAR_MAX_STD_CALC_COUNT * 2 &&
             std_count > RADAR_MAX_STD_CALC_COUNT / 4) {
    bStdMode = true;
    bBlackCarMode = true;
  }

  if (isMaxInvalidRadarData(now_radar_dist)) {
    if (ctx->radar_proc.stableZeroTriggerCount >=
        RADAR_MAX_ZERO_TRIGGER_COUNT) {
      if (Probe_ParkOutTimeout(&ctx->parkstable_zero_radar_timer, 1800,
                               time_now)) {
        LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_REASON, "st 1");
        return true;
      }
      return false;
    }
    if (bStdMode && !bBlackCarMode) {
      for (int i = 0; i < RADAR_MAX_TRIGGER_COUNT; ++i) {
        RadarData_t tmpRadarData;
        int ret = Radar_Trigger(&tmpRadarData);
        if (ret == 0) {
          if (!isMaxInvalidRadarData(&tmpRadarData)) {
            BufferChange(&ctx->radar_proc.buff, &tmpRadarData);
            break;
          }
        }
      }
      now_radar_dist = BufferAt(&ctx->radar_proc.buff, 0);
      if (!now_radar_dist)
        return false;
      if (isMaxInvalidRadarData(now_radar_dist)) {
        ctx->radar_proc.outProbe += 1;

        int outProbeThreshold = useCount + 3;
        if (ctx->radar_proc.outProbe > outProbeThreshold) {
          ctx->radar_proc.stableZeroTriggerCount++;

          LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_REASON, "st 2 %d",
                        ctx->radar_proc.outProbe);
          return true;
        }
        return false;
      }
    } else {
      ctx->radar_proc.stableZeroCount++;
      if (ctx->radar_proc.stableZeroCount >= RADAR_MAX_ZERO_FREE_COUNT) {
        ctx->radar_proc.stableZeroTriggerCount++;
        LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_REASON, "st 3 %d",
                      ctx->radar_proc.stableZeroCount);
        return true;
      }
      return false;
    }
  }
  ctx->radar_proc.stableZeroCount = 0;

  if (isInHeighLevelDist(ctx, now_radar_dist->dist, p)) {
    ctx->radar_proc.outProbe += 1;
    ctx->radar_proc.triggerHistory.triggerCount += 1;
    ctx->radar_proc.triggerHistory.lastTriggerDist = now_radar_dist->dist;
    ctx->radar_proc.triggerHistory.lastTriggerMean = mean;
    ctx->radar_proc.triggerHistory.lastTriggerStd = std;
    ctx->radar_proc.triggerHistory.bObjStableMode = false;
    ctx->radar_proc.isCarChanged = true;
    LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_REASON, "st 4");
    return true;
  }

  if (bStdMode) {
    float diff = now_radar_dist->dist - mean;

    float threshold = RADAR_STD_RATIO * std;
    if (threshold < RADAR_MIN_STD_THRESHOLD) {
      threshold = RADAR_MIN_STD_THRESHOLD;
    }
    threshold *= countRatio;

    log_d("stable trigger, stdmode diff(%.2f) thr(%.2f)", diff, threshold);
    float score_threshold = threshold;
    if (score_threshold > 20.f) {
      score_threshold = 20.f;
    }
    if (diff >= threshold) {
      ctx->radar_proc.stableUnstableCount = 0;
      ctx->radar_proc.outProbe += 1;
      float outScore = 0.f;
      outScore += 2 * (diff / score_threshold - 1.f);
      if (bBlackCarMode) {
        outScore *= 2;
      }

      uint16_t tmpRecord[RADAR_MAX_TRIGGER_COUNT] = {0};
      for (int i = 0; i < RADAR_MAX_TRIGGER_COUNT; ++i) {
        RadarData_t tmpRadarData;
        int ret = Radar_Trigger(&tmpRadarData);
        if (ret == 0) {
          tmpRecord[i] = tmpRadarData.dist;
          if (!isMaxInvalidRadarData(&tmpRadarData)) {
            float tmpDiff = tmpRadarData.dist - mean;
            float nowScroe = 0.f;
            if (tmpDiff >= threshold) {
              nowScroe += 2 * (tmpDiff / score_threshold - 1.f);
            }
            outScore += nowScroe;
            if (bBlackCarMode) {
              outScore += nowScroe;
            }
          } else if (ctx->radar_proc.radarFreeDist == 0 &&
                     tmpRadarData.dist == 0 && invalidRadarCount < 2 &&
                     !bBlackCarMode) {
            outScore += 0.2f;
          }
        }
      }
      if (outScore > 1.f) {
        ctx->radar_proc.triggerHistory.triggerCount += 1;
        ctx->radar_proc.triggerHistory.lastTriggerDist = now_radar_dist->dist;
        ctx->radar_proc.triggerHistory.lastTriggerMean = mean;
        ctx->radar_proc.triggerHistory.lastTriggerStd = std;
        ctx->radar_proc.triggerHistory.bObjStableMode = false;
        LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_REASON,
                      "st 5 %.1f %d %d %d %d %d", outScore, tmpRecord[0],
                      tmpRecord[1], tmpRecord[2], tmpRecord[3], tmpRecord[4]);
        return true;
      }
    } else if (diff <= -threshold) {
      ctx->radar_proc.outProbe += 1;
      float outScore = 0.f;
      outScore += diff / (-score_threshold) - 1.f;

      if (bBlackCarMode) {
        outScore *= 2;
      }
      bool bFindUpSignal = false;
      uint16_t tmpRecord[RADAR_MAX_TRIGGER_COUNT] = {0};
      for (int i = 0; i < RADAR_MAX_TRIGGER_COUNT; ++i) {
        RadarData_t tmpRadarData;
        int ret = Radar_Trigger(&tmpRadarData);
        if (ret == 0) {
          tmpRecord[i] = tmpRadarData.dist;
          if (!isMaxInvalidRadarData(&tmpRadarData)) {
            float tmpDiff = tmpRadarData.dist - mean;
            float nowScroe = 0.f;
            if (tmpDiff <= -threshold) {
              nowScroe += tmpDiff / (-score_threshold) - 1.f;
            } else if (tmpDiff >= threshold) {
              bFindUpSignal = true;
              BufferChange(&ctx->radar_proc.buff, &tmpRadarData);
              nowScroe += 2 * (tmpDiff / threshold - 1.f);
            }
            outScore += nowScroe;
            if (bBlackCarMode) {
              outScore += nowScroe;
            }
          } else if (ctx->radar_proc.radarFreeDist == 0 &&
                     tmpRadarData.dist == 0 && invalidRadarCount < 2 &&
                     !bBlackCarMode) {
            outScore += 0.2f;
          }
        }
      }
      now_radar_dist = BufferAt(&ctx->radar_proc.buff, 0);
      if (outScore > 1.f) {
        if (bFindUpSignal) {
          ctx->radar_proc.triggerHistory.triggerCount += 1;
          ctx->radar_proc.triggerHistory.lastTriggerDist = now_radar_dist->dist;
          ctx->radar_proc.triggerHistory.lastTriggerMean = mean;
          ctx->radar_proc.triggerHistory.lastTriggerStd = std;
          ctx->radar_proc.triggerHistory.bObjStableMode = false;
          LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_REASON,
                        "st 6 %.1f %d %d %d %d %d", outScore, tmpRecord[0],
                        tmpRecord[1], tmpRecord[2], tmpRecord[3], tmpRecord[4]);
          return true;
        } else {
          ctx->radar_proc.stableUnstableCount++;
          if (ctx->radar_proc.stableUnstableCount >= 2) {
            LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_REASON,
                          "st 7 %.1f %d %d %d %d %d", outScore, tmpRecord[0],
                          tmpRecord[1], tmpRecord[2], tmpRecord[3],
                          tmpRecord[4]);
            return true;
          }
          return false;
        }
      }
    } else {
      ctx->radar_proc.stableUnstableCount = 0;
      ctx->radar_proc.outProbe = 0;
    }
    return false;
  } else {
    log_d("stable trigger, not stdmode");
    ctx->radar_proc.stableUnstableCount = 0;
    // 数据不够的时候需要考虑刚停稳就出场的情况,或者黑车很极端的数据情况
    return false;
  }
  return false;
}

static bool isOutParkTriggerEvent(ParkCtx_t *ctx, ParamInfo_t *p) {
  RadarData_t *now_radar_dist = NULL;
  now_radar_dist = BufferAt(&ctx->radar_proc.buff, 0);
  float std = 0.f;
  float mean = 0.f;
  int invalidRadarCount = 0;
  int std_count = 0;
  if (!now_radar_dist)
    return false;

  /*打到雷达高值，立刻触发*/
  if (isInHeighLevelDist(ctx, now_radar_dist->dist, p)) {
    ctx->radar_proc.outStableCount++;
    ctx->radar_proc.isCarChanged = true;
  } else if (isInMiddleLevelDist(ctx, now_radar_dist->dist, p)) {
    /*中间范围，雷达稳定触发*/
    if (getStd(&ctx->radar_proc.buff, ctx->radar_proc.parkStableCount, &std,
               &mean, &invalidRadarCount, &std_count)) {
      if (std < 20) {
        LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_REASON, "out 1 %.2f", std);
        return true;
      } else
        return false;
    } else {
      return false;
    }
  } else if (isMaxInvalidRadarData(now_radar_dist)) {
    /*无效数据不会触发*/
    if (ctx->radar_proc.radarFreeDist == 0) {

      // 如果出场的触发类型不是0触发，这里遇到0值才会+1
      if (ctx->radar_proc.stableZeroTriggerCount == 0) {
        ctx->radar_proc.outStableCount++;
      }
      // if (ctx->radar_proc.outStableCount >= 5) {
      //   // ctx->radar_proc.isCarChanged = true;
      // }
    } else {
      ctx->radar_proc.outStableCount = 0;
    }
  } else {
    ctx->radar_proc.outStableCount = 0;
    return false;
  }
  /*不稳定数据，大于5次触发*/
  if (ctx->radar_proc.outStableCount >= 5) {
    LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_REASON, "out 2 %d",
                  ctx->radar_proc.outStableCount);
    return true;
  }
  return false;
}

static void printTriggerLog(ParkCtx_t *ctx) {
  LOG_TRACE_DEV(LL_KEY, LOG_ID_RADAR_TRIGGER_STAT, "RH:(%d %d %.2f %.2f %d) %d",
                ctx->radar_proc.triggerHistory.triggerCount,
                ctx->radar_proc.triggerHistory.lastTriggerDist,
                ctx->radar_proc.triggerHistory.lastTriggerMean,
                ctx->radar_proc.triggerHistory.lastTriggerStd,
                ctx->radar_proc.triggerHistory.bObjStableMode,
                ctx->radar_proc.radarFreeDist);
}

/*雷达检测*/
static int Probe_Radar(ParkCtx_t *ctx, ParamInfo_t *p, ParamAdvInfo_t *p_adv,
                       uint64_t time_now) {
  int event = 0;
  int ret = 0;
  RadarProc_t *proc = &ctx->radar_proc;
  RadarData_t *last_radar = NULL;
  ret = RadarDetect(ctx, p);
  /*雷达数据测试时没有雷达数据可用则退出*/
#ifdef TESTRADARDATABASE
  if (ret < 0) {
    return TRIG_TYPE_NO;
  }
#else
  /*雷达报错则返回雷达错误报警*/
  event = Probe_RadarFault(&ctx->radar_fault_timer, p, time_now, ret != 0);
  if (event != TRIG_TYPE_NO) {
    return event;
  }
#endif

  last_radar = BufferAt(&proc->buff, 0);
  uint16_t last_dist = last_radar ? last_radar->dist : 0;
  log_d("last dist is %d", last_dist);
  int32_t park_state = GET_ALG_STATE(ctx->park_state);
  if (park_state != ctx->radar_proc.lastParkStatus) {
    // 除了停稳和出场之间切换以外，状态切换时初始化这些参数
    if (((park_state != PARK_TYPE_STABLE) && (park_state != PARK_TYPE_OUT)) ||
        ((ctx->radar_proc.lastParkStatus != PARK_TYPE_STABLE) &&
         (ctx->radar_proc.lastParkStatus != PARK_TYPE_OUT))) {
      ctx->radar_proc.parkStableCount = 0;
      ctx->radar_proc.stableZeroTriggerCount = 0;
      ResetDataHistory(&ctx->radar_proc.triggerHistory);
    }

    // 泊位状态切换时初始化这些参数
    // ctx->radar_proc.inStableCount = 0;
    ctx->radar_proc.stableZeroCount = 0;
    ctx->radar_proc.outStableCount = 0;
    ctx->radar_proc.outProbe = 0;
    ctx->radar_proc.stableUnstableCount = 0;
    ctx->radar_proc.lastParkStatus = park_state;
  }
  ctx->radar_proc.parkStableCount++;
  ctx->radar_proc.newestClock++;
  switch (park_state) {
  case PARK_TYPE_IN: {
    bool bOverHalfTime = false;
    if (Probe_ParkInTimeout(&ctx->parkin_timer,
                            p->info.parking_stable_interval / 2, time_now)) {
      bOverHalfTime = true;
    }
    if (isInParkTriggerEvent(ctx, p, bOverHalfTime)) {
      event |= TRIG_TYPE_PARK;
      ctx->radar_proc.lastEventClock = ctx->radar_proc.newestClock;
      ctx->park_state = park_state | PARK_TYPE_STABLE << 4;
      LOG_TRACE_DEV(LL_KEY, LOG_ID_PARK_STABLE_BY_RADAR,
                    "radar:in to stable(%d)", last_dist);
      printTriggerLog(ctx);
      log_i("PARK state: in to stable");
    } else if (Probe_ParkInTimeout(&ctx->parkin_timer,
                                   p->info.parking_stable_interval * 3 / 4,
                                   time_now) &&
               !isMaxInvalidRadarData(last_radar)) {
      // 提前5秒准备触发，遇到有效数据直接触发，防止触发时运气不好刚好是无效数据
      event |= TRIG_TYPE_PARK;
      ctx->radar_proc.lastEventClock = ctx->radar_proc.newestClock;
      ctx->park_state = park_state | PARK_TYPE_STABLE << 4;
      LOG_TRACE_DEV(LL_KEY, LOG_ID_PARK_STABLE_EARLY,
                    "PARK state: in timeout stable early(%d)", last_dist);

      printTriggerLog(ctx);
      log_i("PARK state: in to stable te");
    } else if (Probe_ParkInTimeout(&ctx->parkin_timer,
                                   p->info.parking_stable_interval, time_now)) {
      event |= TRIG_TYPE_PARK;
      ctx->radar_proc.lastEventClock = ctx->radar_proc.newestClock;
      ctx->park_state = park_state | PARK_TYPE_STABLE << 4;
      LOG_TRACE_DEV(LL_KEY, LOG_ID_PARK_STABLE_BY_TIMEOUT,
                    "PARK state: in timeout stable(%d)", last_dist);

      printTriggerLog(ctx);
      log_i("PARK state: in to stable t");
    } else {
      log_i("PARK state: keep in");
    }
    break;
  }
  case PARK_TYPE_STABLE:
  case PARK_TYPE_OUT: {
    /*停稳后雷达发生变化*/
    if (ctx->park_stable) {
      if (isStableParkTriggerEvent(ctx, p, time_now)) {
        event |= TRIG_TYPE_PARK;
        ctx->radar_proc.lastEventClock = ctx->radar_proc.newestClock;
        ctx->park_state = park_state | PARK_TYPE_OUT << 4;
        LOG_TRACE_DEV(LL_KEY, LOG_ID_PARK_OUT_BY_CHANGE, "stable to out(%d)",
                      last_dist);

        printTriggerLog(ctx);
        log_i("PARK state: stable to out");
      }
    } else {
      /*发生变化后检测是否出场*/
      if (isOutParkTriggerEvent(ctx, p)) {
        event |= TRIG_TYPE_PARK;
        ctx->radar_proc.lastEventClock = ctx->radar_proc.newestClock;
        ctx->park_state = park_state | PARK_TYPE_FREE << 4;
        LOG_TRACE_DEV(LL_KEY, LOG_ID_PARK_FREE_BY_RADAR, "out to free(%d)",
                      last_dist);

        printTriggerLog(ctx);
        log_i("PARK state: out to free");
      } else if (Probe_ParkOutTimeout(&ctx->parkout_timer,
                                      p_adv->info.parking_out_interval / 2,
                                      time_now) &&
                 !isMaxInvalidRadarData(last_radar)) {
        // 提前5秒准备触发，遇到有效数据直接触发，防止触发时运气不好刚好是无效数据
        event |= TRIG_TYPE_PARK;
        ctx->radar_proc.lastEventClock = ctx->radar_proc.newestClock;
        ctx->park_state = park_state | PARK_TYPE_FREE << 4;
        LOG_TRACE_DEV(LL_KEY, LOG_ID_PARK_FREE_EARLY,
                      "PARK state: out timeout out early(%d)", last_dist);

        printTriggerLog(ctx);
      } else if (Probe_ParkOutTimeout(&ctx->parkout_timer,
                                      p_adv->info.parking_out_interval,
                                      time_now)) {
        event |= TRIG_TYPE_PARK;
        ctx->radar_proc.lastEventClock = ctx->radar_proc.newestClock;
        ctx->park_state = park_state | PARK_TYPE_FREE << 4;
        LOG_TRACE_DEV(LL_KEY, LOG_ID_PARK_FREE_BY_TIMEOUT,
                      "PARK state: out timeout out(%d)", last_dist);
        printTriggerLog(ctx);
      } else {
        log_i("PARK state: keep out");
      }
      break;
    }

    break;
  }
  case PARK_TYPE_FREE: {
    if (isFreeParkTriggerEvent(ctx, p)) {
      event |= TRIG_TYPE_PARK;
      ctx->radar_proc.freeUnstableCount = 0;
      ctx->radar_proc.lastEventClock = ctx->radar_proc.newestClock;
      log_i("PARK state: free to in");
      ctx->park_state = park_state | PARK_TYPE_IN << 4;
      LOG_TRACE_DEV(LL_KEY, LOG_ID_PARK_IN_BY_CHANGE, "radar:free to in(%d)",
                    last_dist);
      printTriggerLog(ctx);
    } else {
      log_i("PARK state: keep free");
    }

    break;
  }
  default:
    break;
  }
  return event;
}

static void GetSnapshotParam(SnapshotParam_t p[3], DevTime_t *cur_time,
                             bool *empty, uint32_t *interval) {
  bool find_param = false;
  uint8_t index = 0;

  for (int i = 0; i < 3; i++) {
    if (App_IsTimeInPeriod(p[i].time_begin, p[i].time_end, cur_time)) {
      index = i;
      find_param = true;
      break;
    }
  }
  if (!find_param) {
    *empty = false;
    log_w("can not find snapshot interval period!, use 1800s as default!");
    *interval = 30 * 60;
  }
  *empty = p[index].empty_snapshot_en;
  *interval = p[index].interval;
}

static void SnapshotSetState(ParkCtx_t *ctx) {
  int32_t park_state = GET_ALG_STATE(ctx->park_state);
  switch (park_state) {
  case PARK_TYPE_IN: {
    ctx->park_state = park_state | PARK_TYPE_STABLE << 4;
    break;
  }
  case PARK_TYPE_STABLE:
  case PARK_TYPE_OUT: {
    /*停稳后雷达发生变化*/
    if (ctx->park_stable) {
      ctx->park_state = park_state | PARK_TYPE_OUT << 4;
    } else {
      /*发生变化后检测是否出场*/
      ctx->park_state = park_state | PARK_TYPE_FREE << 4;
    }
    break;
  }
  case PARK_TYPE_FREE: {
    ctx->park_state = park_state | PARK_TYPE_IN << 4;
    break;
  }
  default:
    break;
  }
}

/*秒图检测*/
static int Probe_Snapshot(PollingTimer_t *timer, ParamAdvInfo_t *p_adv,
                          uint64_t time_now, ParkType_e park_state,
                          DevTime_t *cur_time, bool *is_first) {
  bool empty_en = false;
  uint32_t interval = 0;
  park_state = GET_ALG_STATE(park_state);
  /*根据时间获得定时抓拍参数*/
  GetSnapshotParam(p_adv->info.snapshot_param, cur_time, &empty_en, &interval);
  /*根据空位秒图是否使能和停车状态判断是否需要检测秒图*/
  interval = *is_first ? p_adv->info.first_snapshot_interval : interval;
  bool snap_shot = (empty_en) || (park_state != PARK_TYPE_FREE);
  snap_shot = snap_shot && (interval != 0);
  PollingTimer_Enable(timer, snap_shot);
  /*首次秒图使用指定参数，后面的秒图则使用时间段里的参数*/
  PollingTimer_Interval(timer, interval);
  log_d("snap enale: %d empty: %d, first: %d, interval:%d", snap_shot, empty_en,
        *is_first, interval);
  if (PollingTimer_Probe(timer, time_now, snap_shot)) {
    PollingTimer_Restart(timer, time_now);
    /*如果触发了秒图则关闭首次秒图标志位*/
    if (*is_first) {
      *is_first = false;
    }
    return TRIG_TYPE_SNAPSHOT;
  }
  return TRIG_TYPE_NO;
}

/*算法触发状态特别处理*/
static int AlgTriggerProc(TriggerInfo_t *info, bool park_table) {
  uint8_t radar_state = GET_RADAR_STATE(info->park_state);
  uint8_t alg_state = GET_ALG_STATE(info->park_state);
  if ((alg_state == PARK_TYPE_STABLE) || (alg_state == PARK_TYPE_OUT)) {
    if ((info->trig_type & TRIG_TYPE_PARK) != 0) {
      /*雷达触发按照停稳状态修改触发类型状态*/
      alg_state = park_table ? PARK_TYPE_STABLE : PARK_TYPE_OUT;
    } else {
      /*非雷达触发按照停稳状态处理*/
      alg_state = PARK_TYPE_STABLE;
    }
  }
  info->park_state = alg_state + (radar_state << 4);
  return 0;
}

/*保存触发信息*/
static int SaveTrigInfo(TriggerInfo_t *info, int event, ParkType_e park_state,
                        bool isCarChanged, uint16_t radar_dist, ParamInfo_t *p,
                        int emptyThreshold) {
  memset(info, 0, sizeof(TriggerInfo_t));
  info->trig_type = event;
  info->park_state = park_state;
  info->empty_threshold = emptyThreshold /*p->info.radar_exit_dist*/;
  info->is_car_change = isCarChanged;
  info->radar_data = radar_dist;
  log_i("car %s", info->is_car_change ? "changed" : "not changed");
  return 0;
}

static void SaveLogTrigInfo(int event, ParkType_e park_state) {
  log_w("wake soc, reason:|%s%s%s%s%s%s%s%s%s%s%s%s",
        event & TRIG_TYPE_PARK ? "PARK EVENT|" : "",
        event & TRIG_TYPE_PWRON ? "SOC POWER ON|" : "",
        event & TRIG_TYPE_LOW_BATT ? "LOW BATTERY|" : "",
        event & TRIG_TYPE_HEART ? "HEART TIMEOUT|" : "",
        event & TRIG_TYPE_BATT_DOOR ? "BATTERY DOOR OPEN|" : "",
        event & TRIG_TYPE_RESOTRE ? "RESTORE|" : "",
        event & TRIG_TYPE_DEBUG ? "DEBUG|" : "",
        event & TRIG_TYPE_RADAR_FAULT ? "RADAR FAULT|" : "",
        event & TRIG_TYPE_RADAR_COVER ? "RADAR COVER|" : "",
        event & TRIG_TYPE_SNAPSHOT ? "SNAPSHOT|" : "",
        event & TRIG_TYPE_UPDATE ? "UPDATE|" : "",
        event & TRIG_TYPE_PLATE ? "PLATE|" : "");
  if ((event & TRIG_TYPE_PARK) != 0) {
    LOG_TRACE_DEV(LL_KEY, LOG_ID_PARK_STATE, "PARK State: alg:%d, radar:%d",
                  GET_ALG_STATE(park_state), GET_RADAR_STATE(park_state));
  }
}

#if 0
static void radar_data_save_log(RadarProc_t *proc, int num) {
  RadarData_t *r_data = NULL;
  for (int i = 0; i < num; i++) {
    r_data = BufferAt(&proc->buff, i);
    if (r_data) {
      LOG_TRACE_SYS(LL_INFO, LOG_ID_LAST_RADAR, "radar[%d]: dist %d amp %d", i,
                    r_data->dist, r_data->amp);
    }
  }
}
#endif
static int SaveRadarHistory(DataBuff_t *buff, radar_history_t *his) {
  int i = 0;
  his->count = BuffGetCount(buff) > RADAR_HISTORY_BUFF_SIZE
                   ? RADAR_HISTORY_BUFF_SIZE
                   : BuffGetCount(buff);
  for (i = 0; i < his->count; i++) {
    memcpy(his->data + i, BufferAt(buff, i), sizeof(RadarData_t));
  }
  return 0;
}

static int BusWakeSoc(ParkCtx_t *ctx, int event, ParamInfo_t *p) {
  RadarData_t *last_radar = NULL;
  last_radar = BufferAt(&ctx->radar_proc.buff, 0);
  if ((event & TRIG_TYPE_PARK) == 0) {
    if (last_radar && isMaxInvalidRadarData(last_radar)) {
      for (int i = 1; i < 3; ++i) {
        RadarData_t *tmp_radar = BufferAt(&ctx->radar_proc.buff, i);
        if(!tmp_radar) {
          break;
        }
        if (!isMaxInvalidRadarData(tmp_radar)) {
          last_radar = tmp_radar;
        }
      }
    }
  }
  int empty_threshold_base = getThresholdBase(ctx->radar_proc.radarFreeDist, p);
  empty_threshold_base = empty_threshold_base * 0.8f;
  SaveTrigInfo(&g_stTrigInfo, event, ctx->park_state,
               ctx->radar_proc.isCarChanged, last_radar ? last_radar->dist : 0,
               p, empty_threshold_base);
  ctx->cur_trig_type = event;
  AlgTriggerProc(&g_stTrigInfo, ctx->park_stable);
  SaveRadarHistory(&ctx->radar_proc.buff, &g_stRadarData);
  log_d("save %d radar history data", g_stRadarData.count);
  ctx->radar_proc.isLastCarChanged = ctx->radar_proc.isCarChanged;
  ctx->radar_proc.isCarChanged = false;
  ctx->radar_proc.event_count = 0;
  ctx->trig_count++;
  SaveLogTrigInfo(event, ctx->park_state);
  LOG_TRACE_DEV(LL_KEY, LOG_ID_TRIG_TYPE, "trigger reason: %d, count:%d", event,
                ctx->trig_count);
  // radar_data_save_log(&ctx->radar_proc, RADAR_DATA_SAVE_LOG_CNT);
  if (g_cbSocPowFunc) {
    if ((event & TRIG_TYPE_PWRON) == 0) {
      DrvMisc_SetStateLightFlicker(0, 0, 0, 0, false);
    }
    SocWorkStatus_e mode = ((event & TRIG_TYPE_UPDATE) != 0)
                               ? SOC_STATUS_UPGRADE
                               : SOC_STATUS_POWERON;
    g_cbSocPowFunc(ctx, p, mode);
  }
  return 0;
}

static void ResetSocPower(ParkCtx_t *ctx, ParamInfo_t *p) {
  if (g_cbSocPowFunc) {
    g_cbSocPowFunc(ctx, p, SOC_STATUS_POWEROFF);
  }
  SLEEP_MS(2000);
  if (g_cbSocPowFunc) {
    g_cbSocPowFunc(ctx, p, SOC_STATUS_POWERON);
  }
}

static bool Probe_Wdt(PollingTimer_t *timer, ParamInfo_t *p,
                      uint64_t time_now) {
  PollingTimer_Enable(timer, p->info.watchdog_enable);
  PollingTimer_Interval(timer, p->info.watchdog_timeout);
  if (PollingTimer_Probe(timer, time_now, true)) {
    log_w("wdt alarm, watchdog_timeout %ds", p->info.watchdog_timeout);
    PollingTimer_Restart(timer, time_now);
    return true;
  }
  return false;
}

static int BusBusReslProc(ParkCtx_t *ctx, AlgResult_t *resl,
                          uint64_t time_now) {
  // int event = g_stTrigInfo.trig_type;
  // uint8_t park_state = g_stTrigInfo.park_state;
  log_i("alg result: reco: %d, has plate: %d", resl->reco, resl->has_plate);
  uint8_t algReco = (resl->reco >> 4) & 0x0F;
  uint8_t nowReco = resl->reco & 0x0F;

  LOG_TRACE_DEV(LL_KEY, LOG_ID_SOC_ALGRESUL_PROC, "alg reco:%d, apply reco:%d",
                algReco, nowReco);

  ctx->park_state = nowReco > PARK_TYPE_FREE ? PARK_TYPE_FREE : nowReco;
  /*change state machine according to alg result*/
  if (ctx->radar_proc.isLastCarChanged) {
    ctx->radar_proc.isCarChanged = false;
  }
  // ctx->radar_proc.isCarChanged = false;
  PollingTimer_Restart(&ctx->parkstable_zero_radar_timer, time_now);
  switch (nowReco) {
  case PARK_TYPE_IN: {
    ctx->park_stable = false;
    PollingTimer_Restart(&ctx->snapshot_timer, time_now);
    PollingTimer_Restart(&ctx->parkin_timer, time_now);
    ctx->first_snapshot = true;
    ctx->has_plate = resl->has_plate;
    ctx->plate_event = 0;
    LOG_TRACE_DEV(LL_KEY, LOG_ID_RECV_ALG_RESULT, "alg result park in");
    break;
  }
  case PARK_TYPE_STABLE: {
    ctx->park_stable = true;
    ctx->has_plate = resl->has_plate || ctx->has_plate;
    /*停稳无车牌使能车牌确认事件*/
    if (!ctx->has_plate) {
      ctx->plate_event++;
      if (ctx->plate_event == 1) {
        PollingTimer_Restart(&ctx->plate_timer, time_now);
      }
    }
    LOG_TRACE_DEV(LL_KEY, LOG_ID_RECV_ALG_RESULT, "alg result park stable");
    break;
  }
  case PARK_TYPE_OUT: {
    /*只有当上次触发事件为停车事件时才切换停稳状态*/
    if ((g_stTrigInfo.trig_type &= TRIG_TYPE_PARK) != 0) {
      ctx->park_stable = !ctx->park_stable;
    } else if (algReco != nowReco) {
      /*其他报警触发，如果算法结果和雷达结果不一致才切换停稳状态*/
      if (ctx->park_stable) {
        ctx->park_stable = false;
      }
    }
    // fusing_conflicting_proc(&ctx->fusing_proc, nowReco, event, park_state);
    PollingTimer_Restart(&ctx->parkout_timer, time_now);
    LOG_TRACE_DEV(LL_KEY, LOG_ID_RECV_ALG_RESULT, "alg result park out");
    break;
  }
  default:
  case PARK_TYPE_FREE: {
    /*radar misrecognition*/
    ctx->park_stable = false;
    ctx->plate_event = 2;
    LOG_TRACE_DEV(LL_KEY, LOG_ID_RECV_ALG_RESULT, "alg result park free");
    break;
  }
  }
  return 0;
}

static int BusShutDownSoc(ParkCtx_t *ctx, ParamInfo_t *p, uint64_t time_now) {
  PollingTimer_Restart(&ctx->heart_timer, time_now);
  if (g_cbSocPowFunc) {
    g_cbSocPowFunc(ctx, p, SOC_STATUS_POWEROFF);
  }
  VzLog_ResetCount();
  if ((ctx->cur_trig_type & TRIG_TYPE_DEBUG) != 0) {
    ResetSocOnTimeout(ctx);
  }
  return 0;
}

static void CmdQueueProc(ParkCtx_t *ctx, ParamInfo_t *p, OS_MSGQUE q,
                         uint64_t time_now) {
  MSG_RET ret = 0;
  MsgCmd_t msg = {0};
  ret = MSG_QUEUE_RECV(q, &msg, 0);
  if (ret == MSG_RET_OK) {
    switch (msg.id) {
    case CMD_ID_UPDATE: {
      uint8_t update = msg.data[0];
      if (update == 0) {
        ctx->state = PIPE_STATE_ALG;
        LOG_TRACE_DEV(LL_KEY, LOG_ID_EXIT_UPDATE, "exit update state");
      } else {
        ctx->state = PIPE_STATE_UPDATE;
        LOG_TRACE_DEV(LL_KEY, LOG_ID_ENTER_UPDATE, "enter update state");
      }
      break;
    }
    case CMD_ID_FACTORY: {
      uint8_t factory = msg.data[0];
      Param_SetFactoryMode(p, (bool)factory);
      Param_SetDebugMode(p, false);
      log_w("factory mode:%s", factory == true ? "ON" : "OFF");
      Param_Write(p);
      Param_Save();

      if (Param_IsFactoryMode(p)) { // 进入工厂模式
        if (-1 == g_iLastPipeStates) {
          g_iLastPipeStates = ctx->state;
        }
        ctx->state = PIPE_STATE_FACTORY;
      } else { // 退出工厂模式
        // tips: exit factory mode, need force upgrade soc
        if (-1 == g_iLastPipeStates) {
          // nothing
        } else {
          // 这里不能进入主流程，手动升级
          BusShutDownSoc(ctx, p, time_now);
          SLEEP_MS(500);
          BusWakeSoc(ctx, TRIG_TYPE_UPDATE, p);
          g_iLastPipeStates = -1;
          ctx->state = PIPE_STATE_UPDATE;
        }
      }
      break;
    }
    default:
      break;
    }
  }
}

static void CmdQueueProcTrig(ParkCtx_t *ctx, ParamInfo_t *p, OS_MSGQUE q,
                             uint64_t time_now) {
  int ret = 0;
  MsgCmd_t msg = {0};
  ret = MSG_QUEUE_RECV(q, &msg, 0);
  if (ret == MSG_RET_OK) {
    log_i("receive message ok, id: %d", msg.id);
    switch (msg.id) {
    case CMD_ID_ALG_RESULT: {
      AlgResult_t resl = {0};
      memcpy(&resl, msg.data, sizeof(AlgResult_t));

      BusBusReslProc(ctx, &resl, time_now);
      ctx->state = PIPE_STATE_ALG;
      break;
    }
    case CMD_ID_SOC_PWDN: {
      BusShutDownSoc(ctx, p, time_now);
      ctx->state = PIPE_STATE_DETECT;
      break;
    }
    case CMD_ID_KICK_WDT: {
      PollingTimer_Restart(&ctx->wdt_timer, time_now);
      break;
    }
    case CMD_ID_UPDATE: {
      uint8_t update = msg.data[0];
      if (update == 0) {
        ctx->state = PIPE_STATE_ALG;
        LOG_TRACE_DEV(LL_KEY, LOG_ID_EXIT_UPDATE, "exit update state");
      } else {
        ctx->state = PIPE_STATE_UPDATE;
        LOG_TRACE_DEV(LL_KEY, LOG_ID_ENTER_UPDATE, "enter update state");
      }
      break;
    }
    case CMD_ID_FACTORY: {
      uint8_t factory = msg.data[0];
      Param_SetFactoryMode(p, (bool)factory);
      Param_SetDebugMode(p, false);
      log_w("factory mode:%s", factory == true ? "ON" : "OFF");
      Param_Write(p);
      Param_Save();

      if (Param_IsFactoryMode(p)) { // 进入工厂模式
        if (-1 == g_iLastPipeStates) {
          g_iLastPipeStates = ctx->state;
        }
        ctx->state = PIPE_STATE_FACTORY;
      } else { // 退出工厂模式
        // tips: exit factory mode, need force upgrade soc
        if (-1 == g_iLastPipeStates) {
          // nothing
        } else {
          // 这里不能进入主流程，手动升级
          BusShutDownSoc(ctx, p, time_now);
          SLEEP_MS(500);
          BusWakeSoc(ctx, TRIG_TYPE_UPDATE, p);
          g_iLastPipeStates = -1;
          ctx->state = PIPE_STATE_UPDATE;
        }
      }
      break;
    }
    case CMD_ID_DEBUG_MODE: {
      DebugMode_t set_mode = {0};
      memcpy(&set_mode, msg.data, sizeof(DebugMode_t));
      if (set_mode.enable != 0) {
        if ((ctx->cur_trig_type & TRIG_TYPE_DEBUG) == 0) {
          DrvMisc_SetDebugLight();
          ctx->debug_pending = TRIG_TYPE_DEBUG;
        }
        AddSocOnTimeout(ctx, set_mode.time);
        log_w("add debug timeout %d", set_mode.time);
      }
    }
    default:
      break;
    }
  }
}

static int CmdQueueProcUpdate(ParkCtx_t *ctx, ParamInfo_t *p, OS_MSGQUE q,
                              uint64_t time_now) {
  int ret = 0;
  int event = TRIG_TYPE_NO;
  MsgCmd_t msg = {0};
  ret = MSG_QUEUE_RECV(q, &msg, 0);
  if (ret == MSG_RET_OK) {
    switch (msg.id) {
    case CMD_ID_ALG_RESULT: {
      AlgResult_t resl = {0};
      memcpy(&resl, msg.data, sizeof(AlgResult_t));
      BusBusReslProc(ctx, &resl, time_now);
      ctx->state = PIPE_STATE_ALG;
      break;
    }
    case CMD_ID_SOC_PWDN: {
      BusShutDownSoc(ctx, p, time_now);
      SLEEP_MS(1000);
      event = TRIG_TYPE_UPDATE;
      break;
    }
    case CMD_ID_KICK_WDT: {
      PollingTimer_Restart(&ctx->wdt_timer, time_now);
      break;
    }
    case CMD_ID_UPDATE: {
      uint8_t update = msg.data[0];
      if (update == 0) {
        ctx->state = PIPE_STATE_ALG;
        LOG_TRACE_DEV(LL_KEY, LOG_ID_EXIT_UPDATE, "exit update state");
        if (g_cbSocPowFunc) {
          // 退出工厂后需要打开usb
          g_cbSocPowFunc(ctx, p, SOC_STATUS_POWERON);
        }
      } else {
        ctx->state = PIPE_STATE_UPDATE;
        LOG_TRACE_DEV(LL_KEY, LOG_ID_ENTER_UPDATE, "enter update state");
      }
      break;
    }
    case CMD_ID_FACTORY: {
      uint8_t factory = msg.data[0];
      Param_SetFactoryMode(p, (bool)factory);
      Param_SetDebugMode(p, false);
      log_w("factory mode:%s", factory == true ? "ON" : "OFF");
      Param_Write(p);
      Param_Save();

      if (Param_IsFactoryMode(p)) { // 进入工厂模式
        if (-1 == g_iLastPipeStates) {
          g_iLastPipeStates = ctx->state;
        }
        ctx->state = PIPE_STATE_FACTORY;
      } else { // 退出工厂模式
        // tips: exit factory mode, need force upgrade soc
        if (-1 == g_iLastPipeStates) {
          // nothing
        } else {
          // 这里不能进入主流程，手动升级
          BusShutDownSoc(ctx, p, time_now);
          SLEEP_MS(500);
          BusWakeSoc(ctx, TRIG_TYPE_UPDATE, p);
          g_iLastPipeStates = -1;
          ctx->state = PIPE_STATE_UPDATE;
        }
      }
      break;
    }
    default:
      break;
    }
  }
  return event;
}

static int CmdQueueProcFactory(ParkCtx_t *ctx, ParamInfo_t *p, OS_MSGQUE q,
                               uint64_t time_now) {
  int ret = 0;
  int event = TRIG_TYPE_NO;
  MsgCmd_t msg = {0};
  ret = MSG_QUEUE_RECV(q, &msg, 0);
  if (ret == MSG_RET_OK) {
    switch (msg.id) {
    case CMD_ID_SOC_PWDN: {
      log_w("power off SOC,but reboot soc now");
      ResetSocPower(ctx, p);
      break;
    }
    case CMD_ID_FACTORY: {
      uint8_t factory = msg.data[0];
      Param_SetFactoryMode(p, (bool)factory);
      Param_SetDebugMode(p, false);
      log_w("factory mode:%s", factory == true ? "ON" : "OFF");
      Param_Write(p);
      Param_Save();

      if (Param_IsFactoryMode(p)) { // 进入工厂模式
        ctx->state = PIPE_STATE_FACTORY;
      } else { // 退出工厂模式
        // tips: exit factory mode, need force upgrade soc
        // 这里不能进入主流程，手动升级
        log_w("enter factory upgrade");
        BusShutDownSoc(ctx, p, time_now);
        SLEEP_MS(500);
        BusWakeSoc(ctx, TRIG_TYPE_UPDATE, p);
        g_iLastPipeStates = -1;
        ctx->state = PIPE_STATE_UPDATE;
      }
      break;
    }
    case CMD_ID_DEBUG_MODE: {
      DebugMode_t set_mode = {0};
      memcpy(&set_mode, msg.data, sizeof(DebugMode_t));
      if (set_mode.enable != 0) {
        if ((ctx->cur_trig_type & TRIG_TYPE_DEBUG) == 0) {
          DrvMisc_SetDebugLight();
          ctx->debug_pending = TRIG_TYPE_DEBUG;
        }
        AddSocOnTimeout(ctx, set_mode.time);
        log_w("add debug timeout %d", set_mode.time);
      }
    }
    default:
      break;
    }
  }
  return event;
}

// static bool fusing_enter_prob(fusing_proc_t *proc, uint32_t time_now) {
//   if (proc->count < CONFLICT_FUSING_CNT) {
//     return false;
//   }
//   PollingTimer_Restart(&proc->fusing_timer, time_now);
//   proc->count = 0;
//   return true;
// }

/*秒图检测*/
// static bool fusing_timeout_probe(PollingTimer_t *timer, uint32_t time_now) {
//   PollingTimer_Enable(timer, true);
//   PollingTimer_Interval(timer, FUSING_TIMEOUT);
//   if (PollingTimer_Probe(timer, time_now, true)) {
//     PollingTimer_Restart(timer, time_now);
//     return true;
//   }
//   return false;
// }

bool Probe_StateTimeout(ParkCtx_t *ctx, ParamInfo_t *p) {
  uint32_t timeout_count = 0;
  if (ctx->last_state != ctx->state) {
    ctx->state_count = 0;
    return false;
  }
  ctx->state_count++;
  if (ctx->state == PIPE_STATE_UPDATE) {
    timeout_count = UPDATE_TIMEOUT_IN_S / p->info.wakeup_interval;
    if (ctx->state_count > timeout_count) {
      ctx->state_count = 0;
      LOG_TRACE_DEV(LL_KEY, LOG_ID_UPDATE_TIMEOUT, "Update Timeout");
      return true;
    }
  } else if ((ctx->state == PIPE_STATE_TRIG) ||
             (ctx->state == PIPE_STATE_ALG)) {
    if ((ctx->cur_trig_type & TRIG_TYPE_DEBUG) == 0) {
      timeout_count = SOCON_TIMEOUT_IN_S / p->info.wakeup_interval;
    } else {
      timeout_count = ctx->soc_on_timeout / p->info.wakeup_interval;
    }

    if (ctx->state_count > timeout_count) {
      ctx->state_count = 0;
      LOG_TRACE_DEV(LL_KEY, LOG_ID_STATE_TIMEOUT, "SOC ON Timeout");
      return true;
    }
  }
  return false;
}

void BusRadarPowerReset() {
  DrvMisc_SetRadarPower(false);
  SLEEP_MS(1000);
  DrvMisc_SetRadarPower(true);
  SLEEP_MS(1000);
}

void BusRadarInit() {
  BusRadarPowerReset();
  log_w("Factory init radar");
  Radar_AdjustUpgrade();
  SLEEP_MS(2000);
  Radar_InitSetttings();
}

void BusSocOnProc(int trig_type) {
  static bool reset_radar = false;
  if (((trig_type & TRIG_TYPE_RADAR_FAULT) != 0) && (!reset_radar)) {
    reset_radar = true;
    Radar_PowerReset();
    log_e("radar fault, try to reset power");
  }
}

bool Park_Exec(ParkCtx_t *ctx, BusWorkInfo_t *bus_info) {
  bool wake_up = false;
  uint64_t time_now = bus_info->time_stamp;
  bool batt_protect = false;
  int event = TRIG_TYPE_NO;

  if (!Param_IsFactoryMode(&bus_info->param)) {
    g_bFactoryInit = false;
  }
  if (!g_queueCmd) {
    g_queueCmd = MSG_QUEUE_CREATE(MAX_CMD_BUFF_CNT, sizeof(MsgCmd_t));
  }
  ctx->last_state = ctx->state;
  switch (ctx->state) {
  case PIPE_STATE_DETECT: {
    log_i("current state: DETECT, soc is OFF, park state is 0x%02X",
          ctx->park_state);
    CmdQueueProc(ctx, &bus_info->param, g_queueCmd, time_now);
    event |= Probe_DebugMode(ctx, &bus_info->param, time_now);
    event |= Probe_FirstPowerOn(&ctx->first_poweron);
    event |= Probe_Heart(&ctx->heart_timer, &bus_info->param, time_now);

    if (!bus_info->radar_shut) {
      event |=
          Probe_Radar(ctx, &bus_info->param, &bus_info->param_adv, time_now);
      if (event == TRIG_TYPE_NO) {
        while (ctx->radar_proc.parkStableCount < RADAR_MAX_STD_CALC_COUNT * 2) {
          // 泊位状态切换之后，会忽略掉之前的雷达数据，此处连续触发多次，快速补齐雷达数据
          event |= Probe_Radar(ctx, &bus_info->param, &bus_info->param_adv,
                               time_now);
          log_d("quick trigger");
          if (event != TRIG_TYPE_NO) {
            break;
          }
        }
      }
    }
    event |= Probe_Plate(&ctx->plate_timer, &bus_info->param_adv,
                         &ctx->plate_event, time_now);
    event |= Probe_Battdoor(&ctx->battdoor_timer, &bus_info->param, time_now);
    event |= Probe_Snapshot(&ctx->snapshot_timer, &bus_info->param_adv,
                            time_now, ctx->park_state, &bus_info->cur_time,
                            &ctx->first_snapshot);
    /*只有秒图不是停车事件的时候才会手动修改radar状态来进入抓拍逻辑*/
    if (((event & TRIG_TYPE_SNAPSHOT) != 0) &&
        ((event & TRIG_TYPE_PARK) == 0)) {
      SnapshotSetState(ctx);
    }

    event |= Probe_BattLow(&ctx->battlow_timer, &bus_info->param, time_now,
                           &batt_protect);
    if ((event == 0) && batt_protect) {
      ctx->state = PIPE_STATE_LOWBATT;
      LOG_TRACE_DEV(LL_KEY, LOG_ID_ENTER_LOWBATT, "battery low protection");
    }

    break;
  }
  case PIPE_STATE_TRIG:
  case PIPE_STATE_ALG: {
    log_i("current state: %s(%d), soc is ON",
          (ctx->state == PIPE_STATE_TRIG) ? "TRIG" : "ALG", ctx->state_count);
    CmdQueueProcTrig(ctx, &bus_info->param, g_queueCmd, time_now);
    BusSocOnProc(g_stTrigInfo.trig_type);
    if (!bus_info->radar_shut) {
      RadarDetect(ctx, &bus_info->param);
    }
    if (Probe_Wdt(&ctx->wdt_timer, &bus_info->param, time_now)) {
      LOG_TRACE_DEV(LL_KEY, LOG_ID_WATCHDOG, "watchdog timeout");
      ResetSocPower(ctx, &bus_info->param);
    }
    if (Probe_StateTimeout(ctx, &bus_info->param)) {
      BusShutDownSoc(ctx, &bus_info->param, time_now);
      wake_up = false;
      SLEEP_MS(1000);
      ctx->state = PIPE_STATE_DETECT;
      break;
    }
    wake_up = true;
    break;
  }
  case PIPE_STATE_UPDATE: {
    log_i("current state: UPDATE(%d)", ctx->state_count);
    event |= CmdQueueProcUpdate(ctx, &bus_info->param, g_queueCmd, time_now);
    if (Probe_StateTimeout(ctx, &bus_info->param)) {
      BusShutDownSoc(ctx, &bus_info->param, time_now);
      SLEEP_MS(1000);
      ctx->state = PIPE_STATE_DETECT;
      break;
    }
    wake_up = true;
    break;
  }
  case PIPE_STATE_FACTORY: {
    log_i("current state: FACTORY");

    event |= CmdQueueProcFactory(ctx, &bus_info->param, g_queueCmd, time_now);
    if (g_cbSocPowFunc && !g_bFactoryInit &&
        Param_IsFactoryMode(&bus_info->param)) {
      log_i("factory mode, start soc");
      /*关机初始化雷达，避免出现阻塞问题*/
      g_cbSocPowFunc(ctx, &bus_info->param, SOC_STATUS_POWEROFF);
      BusRadarInit();
      DrvMisc_SetFactoryModeLight();
      g_cbSocPowFunc(ctx, &bus_info->param, SOC_STATUS_POWERON);
      g_bFactoryInit = true;
    }
    wake_up = true;
    break;
  }
  case PIPE_STATE_LOWBATT: {
    log_i("current state: BATTERY LOW");
    DrvMisc_SetStateLightFlicker(STATUS_LAMP_RED, 500, 0, 1, false);
    event |= Probe_BattLow(&ctx->battlow_timer, &bus_info->param, time_now,
                           &batt_protect);
    if (!batt_protect) {
      ctx->state = PIPE_STATE_DETECT;
      LOG_TRACE_DEV(LL_KEY, LOG_ID_EXIT_LOWBATT, "exit battery low");
    }
    break;
  }
  default: {
    break;
  }
  }

  if (event != 0) {
    BusWakeSoc(ctx, event, &bus_info->param);
    PollingTimer_Restart(&ctx->wdt_timer, time_now);
    ctx->state =
        (ctx->state != PIPE_STATE_UPDATE) ? PIPE_STATE_TRIG : PIPE_STATE_UPDATE;
    wake_up = true;
  }

  return wake_up;
}

int Park_SetMiscCallback(SocPowerCb pwr_cb) {
  g_cbSocPowFunc = pwr_cb;
  return 0;
}

int Park_GetTriggerType(TriggerInfo_t *info) {
  memcpy(info, &g_stTrigInfo, sizeof(TriggerInfo_t));
  return 0;
}

int Park_SetAlgResult(AlgResult_t *result) {
  MsgCmd_t msg = {0};
  MSG_RET ret = MSG_RET_OK;
  if (!g_queueCmd) {
    return -1;
  }
  msg.id = CMD_ID_ALG_RESULT;
  msg.len = sizeof(AlgResult_t);
  memcpy(msg.data, result, sizeof(AlgResult_t));
  ret = MSG_QUEUE_SEND(g_queueCmd, &msg, 0);
  if (ret != MSG_RET_OK) {
    return -1;
  }
  return 0;
}

int Park_SetSocPowerDown() {
  MSG_RET ret = MSG_RET_OK;
  MsgCmd_t msg = {0};
  if (!g_queueCmd) {
    return -1;
  }
  msg.id = CMD_ID_SOC_PWDN;
  msg.len = 0;
  ret = MSG_QUEUE_SEND(g_queueCmd, &msg, 0);
  if (ret != MSG_RET_OK) {
    return -1;
  }
  return 0;
}

int Park_SetUpdate(uint8_t update) {
  MSG_RET ret = MSG_RET_OK;
  MsgCmd_t msg = {0};
  if (!g_queueCmd) {
    return -1;
  }
  msg.id = CMD_ID_UPDATE;
  msg.len = sizeof(uint8_t);
  msg.data[0] = update;
  ret = MSG_QUEUE_SEND(g_queueCmd, &msg, 0);
  if (ret != MSG_RET_OK) {
    return -1;
  }
  return 0;
}

int Park_SetFactoryMode(uint8_t factory) {
  MSG_RET ret = MSG_RET_OK;
  MsgCmd_t msg = {0};
  if (!g_queueCmd) {
    return -1;
  }
  msg.id = CMD_ID_FACTORY;
  msg.len = sizeof(uint8_t);
  msg.data[0] = factory;
  ret = MSG_QUEUE_SEND(g_queueCmd, &msg, 0);
  if (ret != MSG_RET_OK) {
    return -1;
  }
  return 0;
}

int Park_WdtKick() {
  MSG_RET ret = MSG_RET_OK;
  MsgCmd_t msg = {0};
  if (!g_queueCmd) {
    return -1;
  }
  msg.id = CMD_ID_KICK_WDT;
  msg.len = 0;
  ret = MSG_QUEUE_SEND(g_queueCmd, &msg, 0);
  if (ret != MSG_RET_OK) {
    return -1;
  }
  return 0;
}

int Park_SetDebugMode(DebugMode_t *set_mode) {
  MSG_RET ret = MSG_RET_OK;
  MsgCmd_t msg = {0};
  if (!g_queueCmd) {
    return -1;
  }
  msg.id = CMD_ID_DEBUG_MODE;
  msg.len = sizeof(DebugMode_t);
  memcpy(msg.data, set_mode, sizeof(DebugMode_t));
  ret = MSG_QUEUE_SEND(g_queueCmd, &msg, 0);
  if (ret != MSG_RET_OK) {
    return -1;
  }
  return 0;
}

int Park_GetAllData(ParkCtx_t *ctx, char *data, int limit_cnt) {
  int num = 0;
  num = g_stRadarData.count;
  log_d("radar: total cnt %d ", num);

  RadarData_t *r_data = NULL;
  uint32_t total_size = 0;
  if (num > 0) {
    total_size = num * sizeof(RadarData_t);
    total_size = total_size > limit_cnt ? limit_cnt : total_size;
    num = total_size / sizeof(RadarData_t);
    for (int i = 0; i < num; i++) {
      r_data = &g_stRadarData.data[i];
      memcpy(&data[i * sizeof(RadarData_t)], r_data, sizeof(RadarData_t));
    }
  }
  return total_size;
}

#define MANUAL_REBOOT_REASON "reboot.mask"

int Park_SaveRebootFlag(uint32_t mode) {
  OS_FILE fp = NULL;
  uint32_t reboot_mask = mode;
  fp = OS_FOPEN(MANUAL_REBOOT_REASON, "wb+");
  if (fp) {
    OS_FWRITE(&reboot_mask, sizeof(uint32_t), 1, fp);
    OS_FCLOSE(fp);
  }
  return 0;
}

bool Park_IsRebootFlag(uint32_t *mode) {
  bool is_exist = false;
  OS_FILE fp = NULL;
  fp = OS_FOPEN(MANUAL_REBOOT_REASON, "rb");
  if (fp) {
    is_exist = true;
    OS_FREAD(mode, sizeof(uint32_t), 1, fp);
    OS_FCLOSE(fp);
  }
  return is_exist;
}

int Park_ClearRebootFlag(void) {
  OS_FREMOVE(MANUAL_REBOOT_REASON);
  return 0;
}

int Park_SaveCtx(ParkCtx_t *ctx) {
  OS_FILE fp = NULL;
  fp = OS_FOPEN("hold_bus_ctx.dat", "wb+");
  if (fp) {
    OS_FWRITE(ctx, sizeof(ParkCtx_t), 1, fp);
    OS_FCLOSE(fp);
  }
  return 0;
}

int Park_RestoreCtx(ParkCtx_t *ctx) {
  OS_FILE fp = NULL;
  fp = OS_FOPEN("hold_bus_ctx.dat", "rb");
  if (fp) {
    OS_FREAD(ctx, sizeof(ParkCtx_t), 1, fp);
    OS_FCLOSE(fp);
  }
  if (ctx->state != PIPE_STATE_DETECT) {
    ctx->state = PIPE_STATE_DETECT;
    ctx->first_poweron = true;
  }
  return 0;
}

int Park_GetLastRadar(ParkCtx_t *ctx, RadarData_t *data) {
  memcpy(data, &ctx->last_radar, sizeof(RadarData_t));
  return 0;
}
